function writeDoublePendulumEnergy(T,U)

%This function writes the dynamics file for the double pendulum.

filename = 'doublePendulumEnergy';

comments{1} = ['[ENERGY, KINETIC, POTENTIAL] = ' upper(filename) '(Z,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the energy of a double';
comments{4} = '    pendulum.';
comments{5}  = 'INPUTS: ';
comments{6} = '    z = [4xn] matrix of states.';
comments{7} = '    P = struct of parameters';
comments{8}  = 'OUTPUTS: ';
comments{9}  = '    energy = [1xn] vector of total energy';
comments{10} = '    kinetic = [1xn] vector of kinetic energy';
comments{11} = '    potential = [1xn] vector of potential energy';
comments{12} = ' ';
comments{13} = 'NOTES:';
comments{14} = ['    This file was automatically generated by ' mfilename '.m']; 


params{1} = {'m1','link one mass'};
params{2} = {'m2','link two mass'};
params{3} = {'g ','gravity'};
params{4} = {'l1','link one length'};
params{5} = {'l2','link two length'};
params{6} = {'I1','link one moment of inertia about its center of mass'};
params{7} = {'I2','link two moment of inertia about its center of mass'};
params{8} = {'d1','distance between link one center of mass and parent joint'};
params{9} = {'d2','distance between link two center of mass and parent joint'};

states{1} = {'th1','link one absolute angle'};
states{2} = {'dth1','link one angular rate'};
states{3} = {'th2','link two absolute angle'};
states{4} = {'dth2','link two angular rate'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function [energy, kinetic, potential] = ' filename '(z,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(states)
    fprintf(fid,[states{i}{1} ' = z(' num2str(i) ',:); %%' states{i}{2} '\n']);
end
fprintf(fid,'\n');

fprintf(fid,['kinetic = ' vectorize(char(T)) ';\n']);
fprintf(fid,['potential = ' vectorize(char(U)) ';\n']);
fprintf(fid,'energy = potential + kinetic;\n\n');
fprintf(fid,'end \n');

fclose(fid);

end


